#include <Wire.h>

// PINs
const int encoder_motor_right = 2;
const int encoder_motor_left = 3;

// 6.5 is diameter of wheel
// 20 is encoder step
const float _mywheel_step = (PI*6.5f)/20.0f;

// stock of step :)
long _encoder_step_left = 0, _encoder_step_right = 0;

//////////////
// encoders
void reinit_encoders()
{
  _encoder_step_left = _encoder_step_right = 0;
}
void setup_encoders(void)
{
  pinMode(encoder_motor_right, INPUT);
  pinMode(encoder_motor_left, INPUT);
  reinit_encoders();
  attachInterrupt(0, read_encoder_right, CHANGE);
  attachInterrupt(1, read_encoder_left, CHANGE);  
}
void read_encoder_right(void)
{
  _encoder_step_right++;
}
void read_encoder_left(void)
{
  _encoder_step_left++;
}

void print_encoders(void)
{
  Serial.print("Steps Left=");
  Serial.print(_encoder_step_left, DEC);  
  Serial.print(" Right=");
  Serial.print(_encoder_step_right, DEC);    
  Serial.println();
  Serial.print("Distance Left=");
  Serial.print(((float)_encoder_step_left)*_mywheel_step);  
  Serial.print(" Right=");
  Serial.print(((float)_encoder_step_right)*_mywheel_step);
  Serial.println();
  Serial.print("Average=");
  Serial.print(((float)(_encoder_step_right+_encoder_step_left)/2)*_mywheel_step);
  Serial.println();
}

void setup(void)
{
  Serial.begin(9600);
  setup_encoders();
}

unsigned long _encoders_timing_ = 0;

void loop(void)
{
  unsigned long _time_ = millis();
  if((_time_-_encoders_timing_)>1000)
  {
    _encoders_timing_ = _time_;
    print_encoders();
  }

  if(Serial.available()>0)
  {
    int cmd = Serial.read();
    Serial.print("cmd=");
    Serial.println(cmd);    

    if(cmd == 'r' || cmd == 'R')
    {
      // reinit encoders
      reinit_encoders();
    }
  }
}

